/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import br.ufrn.engcomp.mpc.util.JOptimizerUtils;
import br.ufrn.engcomp.mpc.util.MathCommonsUtils;
import cern.colt.matrix.DoubleMatrix1D;
import cern.colt.matrix.DoubleMatrix2D;
import com.joptimizer.functions.ConvexMultivariateRealFunction;
import com.joptimizer.functions.LinearMultivariateRealFunction;
import com.joptimizer.functions.PDQuadraticMultivariateRealFunction;
import com.joptimizer.optimizers.CvxOptimizer;
import com.joptimizer.optimizers.OptimizationRequest;
import com.joptimizer.optimizers.OptimizationResponse;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;

/**
 *
 * @author Felipe
 */
public class JOptimizerQPSolver implements QPSolver {
    
    @Override
    public RealVector solve(RealMatrix g, RealVector f, double c, RealMatrix S, RealVector a) {
         // Objective function
        DoubleMatrix2D P = JOptimizerUtils.convertRealMatrixToJOptimizer(g);
        DoubleMatrix1D Q = JOptimizerUtils.convertRealVectorToJOptimizer(f);
        
        ConvexMultivariateRealFunction objectiveFunction = new PDQuadraticMultivariateRealFunction(P, Q, c);
        
        //inequalities
        ConvexMultivariateRealFunction[] inequalities = generateInequalities(S, a);

        //optimization problem
        OptimizationRequest or = new OptimizationRequest();
        
        or.f0 = objectiveFunction;
//        or.initialPoint = F1.make(new double[] { 0.1, 0.9});
        or.fi = inequalities;
        or.interiorPointMethod = CvxOptimizer.DEFAULT_INTERIOR_POINT_METHOD;
//        or.A = A;
//        or.b = b;
        or.toleranceFeas = 1.E-0;
        or.tolerance = 1.E-0;
//        or.maxIteration = 10;

        //optimization
        CvxOptimizer opt = new CvxOptimizer();
        
        opt.setOptimizationRequest(or);
        
        OptimizationResponse response;
//        RealVector solution = null;
        
        try {
            response = opt.optimize();
            
            System.out.println("Solução encontrada!!!");
            
            return JOptimizerUtils.convertJOptimizerToRealVector(response.solution);
        } catch (Exception ex) {
//            Logger.getLogger(JavaSSPredictiveController.class.getName()).log(Level.SEVERE, "NÃO foi encontrada uma solução factível para o problema QP.", "");
            System.out.println("Solução NÃO encontrada.");
        }
        
        return MathCommonsUtils.convertMatrixToColumnVector(MathCommonsUtils.zeros(g.getRowDimension(), 1));
    }

    private static ConvexMultivariateRealFunction[] generateInequalities(RealMatrix S, RealVector a) {
        List<ConvexMultivariateRealFunction> inequalitiesList = new ArrayList<ConvexMultivariateRealFunction>();
        
        RealVector A;
        double b;
        LinearMultivariateRealFunction inequality;
        
        for (int i=0; i<S.getRowDimension(); i++) {
            A = S.getRowVector(i);
            b = a.getEntry(i);
            
            inequality = new LinearMultivariateRealFunction(JOptimizerUtils.convertRealVectorToJOptimizer(A), b);
            
            inequalitiesList.add(inequality);
        }
        
//        //if you want x>0 and y>0
//        inequalities[0] = new LinearMultivariateRealFunction(F1.make(new double[]{-1, 0}), 0);
//        inequalities[1] = new LinearMultivariateRealFunction(F1.make(new double[]{0, -1}), 0);
        
        ConvexMultivariateRealFunction[] inequalities = new ConvexMultivariateRealFunction[inequalitiesList.size()];
        
        return inequalitiesList.toArray(inequalities);
    }
    
}
